%% 多导弹三维编队控制
sigmaL = 0; % 领弹的航向角指令
VLC = 275; % 领弹的速度指令
gammaL = 0; % 领弹的滚转角指令
xf_0 = -20;
yf_0 = 50;
zf_0 = -30;
% 指定的相对距离
xf_d = 75;
yf_d = 150;
zf_d = 0;
% 时间常数
tauV = 4;
tauSigma = 1;
tauGamma = 1;
ke1 = diag(1.2,1.2,1.2);
ke2 = diag(0.95,0.95,0.95);
step = 0.01;
%% 状态量xf,yf,zf,sigmaF,gammaF,VF
for t = 0:0.1:60
if t <= 3
    sigmaL = 0;
elif t > 3 && t<=15
    sigmaL = 5*(t-3);
elif  t > 15
    sigmaL = 60;
end
lambda1 = cos(sigmaF)*cos(gammaF);
lambda2 = sin(sigmaF)*sin(gammaF);
lambda3 = cos(sigmaF)*sin(gammaF);
lambda4 = sin(sigmaF)*cos(gammaF);

dXL = VL*cos(sigmaL)*cos(gammaL);
dYL = VL*sin(sigmaL)*cos(gammaL);
dZL = VL*sin(gammaL);
dXF = VF*cos(sigmaF)*cos(gammaF);
dYF = VF*sin(sigmaF)*cos(gammaF);
dZF = VF*sin(gammaF);
dVF = (VFC - VF)/tauV;
dSigmaF = (sigmaFC - sigmaF)/tauSigma;
dGammaF = (gammaFC - gammaF)/tauGamma;
R = [lambda1      -sin(sigmaF)   lambda3;
     lambda4       cos(sigmaF)   lambda2;
     -sin(gammaF)  0             cos(gammaF);
     ];
dR_dSigmaF = [-sin(sigmaF)*cos(gammaF)  -cos(sigmaF)  -sin(sigmaF)*sin(gammaF);
              cos(sigmaF)*cos(gammaF)   -sin(sigmaF)   cos(sigmaF)*sin(gammaF);
              0                          0               0
    ];
dR_dGammaF = [-cos(sigmaF)*sin(gammaF)      0   cos(sigmaF)*cos(gammaF);
              -sin(sigmaF)*sin(gammaF)      0   sin(sigmaF)*cos(gammaF);
              -cos(gammaF)                  0   -sin(gammaF);
     ];
e = R*[xf-xf_d;yf-yf_d;zf-zf_d];
G = [lambda1/tauV       -VF*lambda4/tauSigma    -VF*lambda3/tauGamma;
     lambda4/tauV       VF*lambda1/tauSigma     -VF*lambda2/tauGamma;
     sin(gammaF)/tauV   0                       VF*cos(gammaF)/tauGamma;  
    ];
de = [dXL-dXF;dYL-dYF;dZL-dZF]...
    -dR_dSigmaF*[xf_d;yf_d;zf_d]*dSigmaF...
    -dR_dGammaF*[xf_d;yf_d;zf_d]*dGammaF;
ddXF = dVF*cos(sigmaF)*cos(gammaF) - VF*dSigmaF*sin(sigmaF)*cos(gammaF) - VF*dGammaF*cos(sigmaF)*sin(gammaF);
ddYF = dVF*sin(sigmaF)*cos(gammaF) + VF*dSigmaF*cos(sigmaF)*cos(gammaF) - VF*dGammaF*sin(sigmaF)*sin(gammaF);
ddZF = dVF*sin(gammaF) + VF*cos(gammaF)*dGammaF;
u = G\(- ke1*de - ke2*e);
end